#include "device.h"

int stepFull(int steps_,float freq_, int mode_)
{
 int position;
  if(steps_ > 0)
   position =turnFullForw(steps_, freq_, mode_);
 if(steps_ < 0)
  position =turnFullRev(steps_,freq_,mode_);
 else
  position = 0;
 
 return position;
   
}

void stepmotorInit()
{
	pins[0] = CYREG_PRT0_PC0;
	pins[1] = CYREG_PRT0_PC1;
	pins[2] = CYREG_PRT0_PC2;
	pins[3] = CYREG_PRT0_PC3;
	spolecase = 0; // Bruges til og holde styr på hvilken spole motoren sidst brugte ved Full step er der 4 kombinationer
	spolecaseHalf = 0; //  Bruges til og holde styr på hvilken spole motoren sidst brugte ved half step er der 8 kombinationer
	mode = 1; // Mode 1 motoren acces op  // Mode 0 motoren starter med den definerede Freq
	stepup = 5; // Bruges i mode 1 hvor motoren starter fra 0 hertz og arbejders sig op til en given Freq
	Last = 0; // Holder styr på om der sidst blev kørt frem eller tilbage det er vigtigt da ved 2x frem skal springes 1 spole over
	delay = 0; // gloabal konstant delay som bruges i flere funktioner
	CyPins_SetPinDriveMode(pins[0], PIN_DM_STRONG);
	CyPins_SetPinDriveMode(pins[1], PIN_DM_STRONG);
	CyPins_SetPinDriveMode(pins[2], PIN_DM_STRONG);
	CyPins_SetPinDriveMode(pins[3], PIN_DM_STRONG);
}

int turnWaveForw(int steps_, float freq_, int mode_)
{
	if(Last == 0)
	 	spolecase++;
	int stepstate = 0;
	while(stepstate <= steps_)
	{
			if(mode_ == 1)   // Mode 1 her acceleres motoren op
				{
					if(stepup < freq_)
					{
						stepup += 1/12;
						delay = (1/stepup)*1000;
					}
				}
			else
				{
					delay = (1/freq_)*1000;
				}
			if(spolecase > 3 )
				spolecase = 0;
			if(spolecase < 0)
				spolecase = 3;
			
			switch(spolecase)
			{
				case 0:
				{
					CyPins_SetPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					
				}
				break;
				case 1:
				{
					CyPins_SetPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					
				}
				break;
				case 2:
				{
					CyPins_SetPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					CyPins_ClearPin(pins[1]);
				}
				break;
				case 3:
				{
					CyPins_SetPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					
				}
				break;
			
			}
			spolecase++;
			CyDelay(delay);
			stepstate++;
	}
	stepup = 0;
	Last = 0;
	return (--stepstate);
}

int turnWaveRev(int steps_, float freq_, int mode_)
{
	if(Last == 1)
		++spolecase;
	int stepstate = 0;
	while(stepstate <= steps_)
	{
			if(mode_ == 1)   // Mode 1 her acceleres motoren op
				{
					if(stepup < freq_)
					{
						++stepup;
						delay = (1/stepup)*1000;
						
					}
				}
			else
				{
					delay = (1/freq_)*1000;
				}
			if(spolecase > 3 )
				spolecase = 0;
			if(spolecase < 0)
				spolecase = 3;
			
			switch(spolecase)
			{
				case 0:
				{
					CyPins_SetPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					
				}
				break;
				case 1:
				{
					CyPins_SetPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					
				}
				break;
				case 2:
				{
					CyPins_SetPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					
				}
				break;
				case 3:
				{
					CyPins_SetPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					CyPins_ClearPin(pins[1]);
				}
				break;
				
			
			}
			--spolecase;
			CyDelay(delay);
			++stepstate;
	}
	stepup = 0;
	Last = 1;
	return -(--stepstate);
}

int turnFullForw(int steps_, float freq_, int mode_)
{
	if(Last == 0)
	 spolecase++;
	int stepstate = 0;
	while(stepstate <= steps_)
	{
			if(mode_ == 1)   // Mode 1 her acceleres motoren op
				{
					if(stepup < freq_)
					{
						stepup++;
						delay = (1/stepup)*1000;
					}
				}
			else
				{
					delay = (1/freq_)*1000;
				}
			if(spolecase > 3 )
				spolecase = 0;
			if(spolecase < 0)
				spolecase = 3;
			
			switch(spolecase)
			{
				case 0:
				{
					CyPins_SetPin(pins[0]);
					CyPins_SetPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					
				}
				break;
				case 1:
				{
					CyPins_SetPin(pins[3]);
					CyPins_SetPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					
				}
				break;
				case 2:
				{
					CyPins_SetPin(pins[2]);
					CyPins_SetPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					CyPins_ClearPin(pins[1]);
				}
				break;
				case 3:
				{
					CyPins_SetPin(pins[1]);
					CyPins_SetPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					
				}
				break;
			
			}
			spolecase++;
			CyDelay(delay);
			stepstate++;
	}
	stepup = 0;
	//stepstate = stepstate-1;
	Last = 0;
	return (--stepstate);
}

int turnFullRev(int steps_, float freq_, int mode_)
{
	if(Last == 1)
		spolecase++;
	int stepstate = 0;
	while(stepstate <= steps_)
	{
			if(mode_ == 1)   // Mode 1 her acceleres motoren op
				{
					if(stepup < freq_)
					{
						stepup++;
						delay = (1/stepup)*1000;
						
					}
				}
			else
				{
					delay = (1/freq_)*1000;
				}
			if(spolecase < 0 )
				spolecase = 3;
			if(spolecase > 3)
				spolecase = 0;
			
			switch(spolecase)
			{
				case 0:
				{
					CyPins_SetPin(pins[1]);
					CyPins_SetPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					
				}
				break;
				case 1:
				{
					CyPins_SetPin(pins[0]);
					CyPins_SetPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					
				}
				break;
				case 2:
				{
					CyPins_SetPin(pins[3]);
					CyPins_SetPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					CyPins_ClearPin(pins[2]);
				}
				break;
				case 3:
				{
					CyPins_SetPin(pins[2]);
					CyPins_SetPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					
				}
				break;
			
			}
			spolecase--;
			CyDelay(delay);
			stepstate++;
	}
	//stepstate = stepstate-1;
	Last = 1;
	stepup = 0;
	return -(--stepstate);
} 

int turnHalfForw(int steps_, float freq_, int mode_)
{
	if(Last == 0)
		spolecase++;
		
	int stepstate = 0;
	while(stepstate < steps_)
	{
			if(mode_ == 1)   // Mode 1 her acceleres motoren op
				{
					if(stepup < freq_)
					{
						stepup++;
						delay = (1/stepup)*1000;
						
					}
				}
			else
				{
					delay = (1/freq_)*1000;
				}
			if(spolecaseHalf > 7 )
				spolecaseHalf = 0;
			
			switch(spolecaseHalf)
			{
				case 0:
				{
					CyPins_SetPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					
				}
				break;
				case 1:
				{
					CyPins_SetPin(pins[3]);
					CyPins_SetPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					CyPins_ClearPin(pins[2]);
				}
				break;
				case 2:
				{
					CyPins_SetPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					
				}
				break;
				case 3:
				{
					CyPins_SetPin(pins[2]);
					CyPins_SetPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					
				}
				break;
				case 4:
				{
					CyPins_SetPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					
				}
				break;
				case 5:
				{
					CyPins_SetPin(pins[1]);
					CyPins_SetPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					CyPins_ClearPin(pins[0]);
				}
				break;
				case 6:
				{
					CyPins_SetPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					
				}
				break;
				case 7:
				{
					CyPins_SetPin(pins[0]);
					CyPins_SetPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					
				}
				break;
			
			}
			spolecaseHalf++;
			CyDelay(delay);
			stepstate++;
	}
	stepup = 0;
	Last = 0;
	return (--stepstate);
}

int turnHalfRev(int steps_, float freq_, int mode_)
{
	if(Last == 1)
		spolecase++;
	int stepstate = 0;
	while(stepstate < steps_)
	{
			if(mode_ == 1)   // Mode 1 her acceleres motoren op
				{
					if(stepup < freq_)
					{
						stepup++;
						delay = (1/stepup)*1000;
						
					}
				}
			else
				{
					delay = (1/freq_)*1000;
				}
			if(spolecaseHalf < 0 )
				spolecaseHalf = 7;
			
			switch(spolecaseHalf)
			{
				case 0:
				{
					CyPins_SetPin(pins[0]);
					CyPins_SetPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					
				}
				break;
				case 1:
				{
					CyPins_SetPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					
				}
				break;
				case 2:
				{
					CyPins_SetPin(pins[3]);
					CyPins_SetPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					CyPins_ClearPin(pins[2]);
				}
				break;
				case 3:
				{
					CyPins_SetPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					
				}
				break;
				case 4:
				{
					CyPins_SetPin(pins[2]);
					CyPins_SetPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					
				}
				break;
				case 5:
				{
					CyPins_SetPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					CyPins_ClearPin(pins[1]);
					
				}
				break;
				case 6:
				{
					CyPins_SetPin(pins[1]);
					CyPins_SetPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					CyPins_ClearPin(pins[0]);
				}
				break;
				case 7:
				{
					CyPins_SetPin(pins[1]);
					CyPins_ClearPin(pins[2]);
					CyPins_ClearPin(pins[3]);
					CyPins_ClearPin(pins[0]);
					
				}
				break;
			
			
			}
			spolecaseHalf--;
			CyDelay(delay);
			stepstate++;
	}
	stepup = 0;
	Last = 1;	 
	return -(--stepstate);
}